package edu.wpi.first.wpilibj.defaultCode;

import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Joystick;
import java.util.Date;
import java.util.Timer;
import java.util.TimerTask;

/**
 *
 * @author Wesley
 */
public class Controller extends TimerTask implements IGingersnapCB {

    private Joystick driverJoy;
    private Joystick rotationJoy;
    private Joystick armJoy;
    private MinibotDeploy miniBot;
    private DriveTrain dt;
    private Timer threadTimer;
    private Lifter lifter;
    private Arm arm;
    private Wrist wrist;
    private Compressor compressor;
    private boolean isInitialized = false;

    Controller() {
    }

    public void initialize() {
        System.out.println("Controller::initialize");
        if (!isInitialized) {
            driverJoy = new Joystick(Constants.JOYSTICK_DRIVE);
            rotationJoy = new Joystick(Constants.JOYSTICK_ROTATION);
            armJoy = new Joystick(Constants.JOYSTICK_ARM);
           dt = new DriveTrain(driverJoy, rotationJoy);
//        compressor = new Compressor(Constants.MODULE_NUM,
//                                    Constants.COMPRESOR_PRESSURE_CHNL,
//                                    Constants.MODULE_NUM,
//                                    Constants.COMPRESOR_RELAY_CHNL);
            //       compressor.start();
            threadTimer = new Timer();

            // revisit constants for conflict

            if ( Constants.USE_THREE_JOYSTICKS)
            {
//            arm = new Arm(armJoy);
//            wrist = new Wrist(armJoy);
           //   lifter = new Lifter(armJoy);
            }
            else
            {
//            arm = new Arm(rotationJoy);
//            wrist = new Wrist(rotationJoy);
//            lifter = new Lifter(rotationJoy);
            }
//            miniBot = new MinibotDeploy(driverJoy, this);




            // WHY ISN"T Thread starting ???
           // threadTimer.schedule(dt, new Date(), 10);

            //TODO: Schedule lifter, arm, wrist controllers here
            //  threadTimer.schedule(lifter, new Date(), 10);
            //  threadTimer.schedule(arm, new Date(), 10);
            //  threadTimer.schedule(wrist, new Date(), 10);
            isInitialized = true;
        }
    }

    public void run() {
        if (DriverStation.getInstance().isAutonomous()) {
            driveAuto();
        } else {
            driveTele();
        }
    }

    private void driveAuto() {
        //nothing for now
    }

    private void driveTele() {

        //
        System.out.println("Controller::driveTele: ");
        //     System.out.println("YVal: " + yVal);
        double rot = rotationJoy.getTwist();
        if ( isInitialized() )
        {
            dt.run();
         //   lifter.run();
        }
    }

    private boolean isSnapButtonPressed() {
        if (driverJoy.getRawButton(Constants.SNAP_0_BUTTON_NUM)
                || driverJoy.getRawButton(Constants.SNAP_90_BUTTON_NUM)
                || driverJoy.getRawButton(Constants.SNAP_180_BUTTON_NUM)
                || driverJoy.getRawButton(Constants.SNAP_270_BUTTON_NUM)) {
            return true;
        }

        return false;
    }

    /**
     * Determines if a snap button is pressed and if it is, return the angle
     * of the button that is pressed
     * @return angle
     */
    private int getSnapAngle() {
        if (driverJoy.getRawButton(Constants.SNAP_0_BUTTON_NUM)) {
            return 0;
        } else if (driverJoy.getRawButton(Constants.SNAP_90_BUTTON_NUM)) {
            return 90;
        } else if (driverJoy.getRawButton(Constants.SNAP_180_BUTTON_NUM)) {
            return 180;
        } else if (driverJoy.getRawButton(Constants.SNAP_270_BUTTON_NUM)) {
            return 270;
        } else {
            return -1;
        }
    }

    public void zeroWheels() {
        dt.zeroWheels();
    }

    public boolean isInitialized() {
        return isInitialized;
    }

    public void stopDrive() {
        dt.setIsDriveable(false);
    }

    public void resumeDrive() {
        dt.setIsDriveable(true);
    }
}
